      
      
      
#include "ros/ros.h"
#include<ros/time.h> 
 #include <ros/duration.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <vector>
#include <iostream>
#include <signal.h>
#include <webots_ros/get_bool.h>
#include <webots_ros/get_float.h>
#include <webots_ros/get_float_array.h>
#include <webots_ros/get_int.h>
#include <webots_ros/get_string.h>
#include <webots_ros/get_uint64.h>
#include <webots_ros/get_urdf.h>
#include <webots_ros/set_bool.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_float_array.h>
#include <webots_ros/set_int.h>
#include <webots_ros/set_string.h>
#include <webots_ros/Int32Stamped.h>
#include <webots_ros/RecognitionObject.h>
#include <webots_ros/RecognitionObjects.h>
#include <webots_ros/StringStamped.h>
#include <webots_ros/Float64Stamped.h>
#include<std_msgs/Float64MultiArray.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PointStamped.h>
#include<geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/WrenchStamped.h>
#include <sensor_msgs/Illuminance.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Range.h>
#include <std_msgs/String.h>

using namespace std;

////////tiny_math////////////////////////////////////////////////
typedef struct {
  double u;
  double v;
} Vector2;

typedef struct {
  double u;
  double v;
  double w;
} Vector3;

typedef struct {
  Vector3 a;
  Vector3 b;
  Vector3 c;
} Matrix33;

///////////ros ////////////////////////////////////////////

static int model_count;
static vector<string> model_list;
static vector<unsigned char> imageColor;
static vector<float> imageRangeFinder;
static double compassValues[3] = {0, 0, 0};
static double GPSValues[3] = {0, 0, 0};


static bool callbackCalled = false;
static bool callbackCalled_gps = false;
static bool callbackCalled_compass = false;
static bool callbackCalled_cameras = false;
static bool callbackCalled_recognition = false;
static bool callbackCalled_rangefinder = false;

ros::NodeHandle *n;
string model_name="my_robot";

ros::ServiceClient time_step_client;          //时钟通讯客户端
webots_ros::set_int time_step_srv;            //时钟服务数据

ros::ServiceClient get_time_client;          //时钟通讯客户端
webots_ros::get_float get_time_srv; 

ros::ServiceClient set_velocity_client;
webots_ros::set_float set_velocity_srv;

ros::ServiceClient set_position_client;   
webots_ros::set_float set_position_srv;  


////////base parameters//////////////////////////////////////////////

#define TIME_STEP 16
#define SPEED 4.0
#define MAX_SPEED 0.3
#define SPEED_INCREMENT 0.05
#define DISTANCE_TOLERANCE 0.001
#define ANGLE_TOLERANCE 0.001

// robot geometry
#define WHEEL_RADIUS 0.05
#define LX 0.228  // longitudinal distance from robot's COM to wheel [m].
#define LY 0.158  // lateral distance from robot's COM to wheel [m].

// stimulus coefficients
#define K1 3.0
#define K2 1.0
#define K3 1.0

// camera parameters
#define FOV 1.5708
#define X_ 640
#define Y_ 380


typedef struct {
  Vector2 v_target;
  double alpha;
  bool reached;
} goto_struct;

ros::ServiceClient wheels_v[4],wheels_p[4];
static goto_struct goto_data;

static double robot_vx = 0.0;
static double robot_vy = 0.0;
static double robot_omega = 0.0;

double distance_arm0_platform = 0.2;
double distance_arm0_robot_center = 0.189;
double distance_origin_platform = 2.5;
double delta=2.111;

//////////gripper geometry////////////////////////////////
#define LEFT 0
#define RIGHT 1

#define MIN_POS 0.0
#define MAX_POS 0.025
#define OFFSET_WHEN_LOCKED 0.021

ros::ServiceClient fingers_v[2],fingers_p[2];

//////////arm parameters//////////////////////////////////

enum Height {
  ARM_BACK_PLATE_LOW,
  ARM_BACK_PLATE_HIGH,
  ARM_RESET,
  ARM_FRONT_CARDBOARD_BOX,
  ARM_HANOI_PREPARE,
  ARM_FRONT_PLATE,
  ARM_FRONT_FLOOR,
  ARM_MAX_HEIGHT
};

enum Orientation {
  ARM_BACK_RIGHT,
  ARM_RIGHT,
  ARM_FRONT_RIGHT,
  ARM_FRONT,
  ARM_FRONT_LEFT,
  ARM_LEFT,
  ARM_BACK_LEFT,
  ARM_MAX_SIDE
};

enum Arm { ARM1, ARM2, ARM3, ARM4, ARM5 };

ros::ServiceClient arm_elements_v[5],arm_elements_p[5];

static enum Height current_height = ARM_RESET;
static enum Orientation current_orientation = ARM_FRONT;

enum Height new_height;
enum Orientation new_orientation;

string arm_name[5]={"arm1", "arm2", "arm3", "arm4", "arm5"};


////////////////////////////////////////////////////////////////
//                         ros
////////////////////////////////////////////////////////////////

/*******************************************************
* Function name ：modelNameCallback
* Description   ：获得所有模型名字
* Parameter     ：
* Return        ：无
**********************************************************/
void modelNameCallback(const std_msgs::String::ConstPtr &name) {
  model_count++;
  model_list.push_back(name->data);
  ROS_INFO("Model #%d: %s.", model_count, model_list.back().c_str());
  callbackCalled = true;
}
/*******************************************************
* Function name ：gpsCallback
* Description   ：GPS回调函数，获取GPS消息
* Parameter     ：
        @ values   GPS位置消息值
* Return        ：无
**********************************************************/
void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values)
{
  GPSValues[0] = values->point.x;// x坐标
  GPSValues[1] = values->point.y;// y坐标
  GPSValues[2] = values->point.z;// z坐标
  callbackCalled_gps = true;
  callbackCalled = true;
}
/*******************************************************
* Function name ：cameraCallback
* Description   ：camera回调函数，获取image
* Parameter     ：
        @ values   image
* Return        ：无
**********************************************************/
void cameraCallback(const sensor_msgs::Image::ConstPtr &values) {
  int i = 0;
  imageColor.resize(values->step * values->height);
  for (std::vector<unsigned char>::const_iterator it = values->data.begin(); it != values->data.end(); ++it) {
    imageColor[i] = *it;
    i++;
  }
  callbackCalled = true;
  callbackCalled_cameras = true;
}


/*******************************************************
* Function name ：rangeFinderCallback
* Description   ：rangeFinderCallback回调函数，获取image
* Parameter     ：
        @ values   image
* Return        ：无
**********************************************************/
void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image) {
  int size = image->width * image->height;
  imageRangeFinder.resize(size);
  const float *depth_data = reinterpret_cast<const float *>(&image->data[0]);
  for (int i = 0; i < size; ++i)
    {
    imageRangeFinder[i] = depth_data[i];
    }
  callbackCalled = true;
  callbackCalled_rangefinder=true;
}

// 49 47 39;
double findobject_id=39;
Vector2 findobject_point;
void yolov5Callback(const std_msgs::Float64MultiArray &value)
{
    int l=value.data[0];

    double best_score=-1;
    for(int i=1;i<=l;i++)
    {
      if(abs(value.data[i]-findobject_id)<0.01)
      {
          if(value.data[i-1]>best_score)
          {
            best_score=value.data[i-1];
            findobject_point.u=(value.data[i-3]+value.data[i-5])/2;
            findobject_point.v=(value.data[i-2]+value.data[i-4])/2;
      }}
    }
   
    if(best_score!=-1)
    {callbackCalled_recognition = true;
    callbackCalled = true;
    }
}

Vector3 world_point;
void xyd_to_xyz_(int x_pic,int y_pic )
{
  int num=x_pic+y_pic*X_;
  cout<<x_pic<<" "<<y_pic<<endl;
  double d_=imageRangeFinder[num-1];
  cout<<"d"<<d_<<endl;
  double dx_=X_/2-x_pic;
  double dy_=Y_/2-y_pic;
  double kxyd=tan(FOV/2);
  world_point.v=2*kxyd*d_*dx_/X_;
  world_point.w=2*kxyd*d_*dy_/Y_;
  world_point.u=d_;
}
/*******************************************************
* Function name ：compassCallback
* Description   ：compass回调函数，获取compass消息
* Parameter     ：
        @ values   compass消息值
* Return        ：无
**********************************************************/
void compassCallback(const sensor_msgs::MagneticField::ConstPtr &values) {
  compassValues[0] = values->magnetic_field.x;
  compassValues[1] = values->magnetic_field.y;
  compassValues[2] = values->magnetic_field.z;

  ROS_INFO("Compass values are x=%f y=%f z=%f (time: %d:%d).", compassValues[0], compassValues[1], compassValues[2],
           values->header.stamp.sec, values->header.stamp.nsec);
  callbackCalled = true;
  callbackCalled_compass = true;
}


/*******************************************************
* Function name ：quit
* Description   ：退出函数
* Parameter     ：
        @sig   信号
* Return        ：无
**********************************************************/
void quit(int sig) {
  ROS_INFO("User stopped the '/robot' node.");
  time_step_srv.request.value = 0; 
  time_step_client.call(time_step_srv); 
  ros::shutdown();
  exit(0);
}

////////////////////////////////////////////////////////////////
//                         webots
////////////////////////////////////////////////////////////////

webots_ros::set_float set_float_srv;

bool wb_motor_set_velocity(ros::ServiceClient &device, double velocity)
{
  set_float_srv.request.value = velocity;
  device.call(set_float_srv);
  return set_float_srv.response.success;
}

bool wb_motor_set_position(ros::ServiceClient &device, double position)
{
  set_float_srv.request.value = position;
  device.call(set_float_srv);
  return set_float_srv.response.success;
}


////////////////////////////////////////////////////////////////
//                         tiny_math
////////////////////////////////////////////////////////////////

void vector3_set_values(Vector3 *vect, double u, double v, double w) {
  vect->u = u;
  vect->v = v;
  vect->w = w;
}

void matrix33_set_values(Matrix33 *m, double au, double av, double aw, double bu, double bv, double bw, double cu, double cv,
                         double cw) {
  vector3_set_values(&(m->a), au, av, aw);
  vector3_set_values(&(m->b), bu, bv, bw);
  vector3_set_values(&(m->c), cu, cv, cw);
}

void matrix33_set_identity(Matrix33 *m) {
  matrix33_set_values(m, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
}

void matrix33_mult_vector3(Vector3 *res, const Matrix33 *m, const Vector3 *v) {
  res->u = m->a.u * v->u + m->b.u * v->v + m->c.u * v->w;
  res->v = m->a.v * v->u + m->b.v * v->v + m->c.v * v->w;
  res->w = m->a.w * v->u + m->b.w * v->v + m->c.w * v->w;
}

double vector2_norm(const Vector2 *v) {
  return sqrt(v->u * v->u + v->v * v->v);
}

void vector2_minus(Vector2 *v, const Vector2 *v1, const Vector2 *v2) {
  v->u = v1->u - v2->u;
  v->v = v1->v - v2->v;
}

double vector2_angle(const Vector2 *v1, const Vector2 *v2) {
  return atan2(v2->v, v2->u) - atan2(v1->v, v1->u);
}

double bound(double v, double a, double b) {
  return (v > b) ? b : (v < a) ? a : v;
}


////////////////////////////////////////////////////////////////
//                         gripper
////////////////////////////////////////////////////////////////

void gripper_init() {
  
  ros::ServiceClient set_velocity_client,set_position_client;

  set_velocity_client = n->serviceClient<webots_ros::set_float>(model_name + "/finger1/set_velocity");
  set_position_client = n->serviceClient<webots_ros::set_float>(model_name + "/finger1/set_position");
  fingers_v[LEFT] = set_velocity_client;
  fingers_p[LEFT] = set_position_client;

  if (wb_motor_set_velocity(fingers_v[LEFT],0.05) && wb_motor_set_position(fingers_p[LEFT],MAX_POS))
    ROS_INFO("Finger1 set to 0.03,max.");
  else
    ROS_ERROR("Failed to call service set on Finger1 motor.");

  set_velocity_client = n->serviceClient<webots_ros::set_float>(model_name + "/finger2/set_velocity");
  set_position_client = n->serviceClient<webots_ros::set_float>(model_name + "/finger2/set_position");
  fingers_v[RIGHT] = set_velocity_client;
  fingers_p[RIGHT] = set_position_client;

  if (wb_motor_set_velocity(fingers_v[RIGHT],0.05) && wb_motor_set_position(fingers_p[RIGHT],MAX_POS))
    ROS_INFO("Finger2 set to 0.03,max.");
  else
    ROS_ERROR("Failed to call service set on Finger2 motor.");

}

void gripper_grip() {
  wb_motor_set_position(fingers_p[LEFT],MIN_POS);
  wb_motor_set_position(fingers_p[RIGHT],MIN_POS);
}

void gripper_release() {
  wb_motor_set_position(fingers_p[LEFT],MAX_POS);
  wb_motor_set_position(fingers_p[RIGHT],MAX_POS);
}

void gripper_set_gap(double gap) {
  double v = bound(0.5 * (gap - OFFSET_WHEN_LOCKED), MIN_POS, MAX_POS);

  wb_motor_set_position(fingers_p[LEFT],v);
  wb_motor_set_position(fingers_p[RIGHT],v);
}


////////////////////////////////////////////////////////////////
//                         ARM
////////////////////////////////////////////////////////////////

void arm_set_height(enum Height height) {
  switch (height) {
    case ARM_FRONT_FLOOR:
      wb_motor_set_position(arm_elements_p[ARM2], -0.97);
      wb_motor_set_position(arm_elements_p[ARM3], -1.55);
      wb_motor_set_position(arm_elements_p[ARM4], -0.61);
      wb_motor_set_position(arm_elements_p[ARM5], 0.0);
      break;
    case ARM_FRONT_PLATE:
      wb_motor_set_position(arm_elements_p[ARM2], -0.62);
      wb_motor_set_position(arm_elements_p[ARM3], -0.98);
      wb_motor_set_position(arm_elements_p[ARM4], -1.53);
      wb_motor_set_position(arm_elements_p[ARM5], 0.0);
      break;
    case ARM_FRONT_CARDBOARD_BOX:
      wb_motor_set_position(arm_elements_p[ARM2], 0.0);
      wb_motor_set_position(arm_elements_p[ARM3], -0.77);
      wb_motor_set_position(arm_elements_p[ARM4], 0);
      wb_motor_set_position(arm_elements_p[ARM5], 0.0);
      break;
    case ARM_RESET:
      wb_motor_set_position(arm_elements_p[ARM2], 1.57);
      wb_motor_set_position(arm_elements_p[ARM3], -2.635);
      wb_motor_set_position(arm_elements_p[ARM4], 1.78);
      wb_motor_set_position(arm_elements_p[ARM5], 0.0);
      break;
    case ARM_BACK_PLATE_HIGH:
      wb_motor_set_position(arm_elements_p[ARM2], 0.678);
      wb_motor_set_position(arm_elements_p[ARM3], 0.682);
      wb_motor_set_position(arm_elements_p[ARM4], 1.74);
      wb_motor_set_position(arm_elements_p[ARM5], 0.0);
      break;
    case ARM_BACK_PLATE_LOW:
      wb_motor_set_position(arm_elements_p[ARM2], 0.92);
      wb_motor_set_position(arm_elements_p[ARM3], 0.42);
      wb_motor_set_position(arm_elements_p[ARM4], 1.78);
      wb_motor_set_position(arm_elements_p[ARM5], 0.0);
      break;
    case ARM_HANOI_PREPARE:
      wb_motor_set_position(arm_elements_p[ARM2], -0.4);
      wb_motor_set_position(arm_elements_p[ARM3], -1.2);
      wb_motor_set_position(arm_elements_p[ARM4], -M_PI_2);
      wb_motor_set_position(arm_elements_p[ARM5], M_PI_2);
      break;
    default:
      fprintf(stderr, "arm_height() called with a wrong argument\n");
      return;
  }
  current_height = height;
}

void arm_set_orientation(enum Orientation orientation) {
  switch (orientation) {
    case ARM_BACK_LEFT:
      wb_motor_set_position(arm_elements_p[ARM1], -2.949);
      break;
    case ARM_LEFT:
      wb_motor_set_position(arm_elements_p[ARM1], -M_PI_2);
      break;
    case ARM_FRONT_LEFT:
      wb_motor_set_position(arm_elements_p[ARM1], -0.2);
      break;
    case ARM_FRONT:
      wb_motor_set_position(arm_elements_p[ARM1], 0.0);
      break;
    case ARM_FRONT_RIGHT:
      wb_motor_set_position(arm_elements_p[ARM1], 0.2);
      break;
    case ARM_RIGHT:
      wb_motor_set_position(arm_elements_p[ARM1], M_PI_2);
      break;
    case ARM_BACK_RIGHT:
      wb_motor_set_position(arm_elements_p[ARM1], 2.949);
      break;
    default:
      fprintf(stderr, "arm_set_side() called with a wrong argument\n");
      return;
  }
  current_orientation = orientation;
}

void arm_init() {
  ros::ServiceClient set_velocity_client,set_position_client;
  
  for(int i = 0; i <5;i++)
  {
    set_velocity_client = n->serviceClient<webots_ros::set_float>(model_name + "/"+arm_name[i]+"/set_velocity");
    set_position_client = n->serviceClient<webots_ros::set_float>(model_name + "/"+arm_name[i]+"/set_position");
    arm_elements_v[i]=set_velocity_client;
    arm_elements_p[i]=set_position_client;
  }
  if(wb_motor_set_velocity(arm_elements_v[ARM2], 0.5))
    ROS_INFO("arm2 set to 0.5,none ");
  else
    ROS_ERROR("Failed to call service set on arm2 motor.");

  arm_set_height(ARM_RESET);
  arm_set_orientation(ARM_FRONT);
}

void arm_reset() {
  arm_set_height(ARM_RESET);
  arm_set_orientation(ARM_FRONT);
}

void arm_increase_height() {
  new_height = Height(current_height + 1);
  // Prevents from going beyond index.
  if (new_height >= ARM_MAX_HEIGHT)
    new_height = Height(ARM_MAX_HEIGHT - 1);

  // Prevents self-colliding poses.
  if (new_height == ARM_FRONT_FLOOR) {
    if (current_orientation == ARM_BACK_LEFT || current_orientation == ARM_BACK_RIGHT)
      new_height = current_height;
  }

  arm_set_height(new_height);
}

void arm_decrease_height() {
  new_height = Height(current_height - 1);
  if ((int)new_height < 0)
    new_height = Height(0);
  arm_set_height(new_height);
}

void arm_increase_orientation() {
  new_orientation = Orientation(current_orientation + 1);

  // Prevents from going beyond index.
  if (new_orientation >= ARM_MAX_SIDE)
    new_orientation = Orientation(ARM_MAX_SIDE - 1);

  // Prevents self-colliding poses.
  if (new_orientation == ARM_BACK_LEFT) {
    if (current_height == ARM_FRONT_FLOOR)
      new_orientation = current_orientation;
  }

  arm_set_orientation(new_orientation);
}

void arm_decrease_orientation() {
  new_orientation = Orientation(current_orientation - 1);

  // Prevents from going beyond index.
  if ((int)new_orientation < 0)
    new_orientation = Orientation(0);

  // Prevents self-colliding poses.
  if (new_orientation == ARM_BACK_RIGHT) {
    if (current_height == ARM_FRONT_FLOOR)
      new_orientation = current_orientation;
  }

  arm_set_orientation(new_orientation);
}

void arm_set_sub_arm_rotation(enum Arm arm, double radian) {
  wb_motor_set_position(arm_elements_p[arm], radian);
}

double arm_get_sub_arm_length(enum Arm arm) {
  switch (arm) {
    case ARM1:
      return 0.253;
    case ARM2:
      return 0.155;
    case ARM3:
      return 0.135;
    case ARM4:
      return 0.081;
    case ARM5:
      return 0.105;
  }
  return 0.0;
}

void arm_ik(double x, double y, double z) {
  double y1 = sqrt(x * x + y * y);
  double z1 = z + arm_get_sub_arm_length(ARM4) + arm_get_sub_arm_length(ARM5) - arm_get_sub_arm_length(ARM1);

  double a = arm_get_sub_arm_length(ARM2);
  double b = arm_get_sub_arm_length(ARM3);
  double c = sqrt(y1 * y1 + z1 * z1);

  double alpha = -asin(x / y1);
  double beta = -(M_PI_2 - acos((a * a + c * c - b * b) / (2.0 * a * c)) - atan(z1 / y1));
  double gamma = -(M_PI - acos((a * a + b * b - c * c) / (2.0 * a * b)));
  double delta = -(M_PI + (beta + gamma));
  double epsilon = M_PI_2 + alpha;

  wb_motor_set_position(arm_elements_p[ARM1], alpha);
  wb_motor_set_position(arm_elements_p[ARM2], beta);
  wb_motor_set_position(arm_elements_p[ARM3], gamma);
  wb_motor_set_position(arm_elements_p[ARM4], delta);
  wb_motor_set_position(arm_elements_p[ARM5], epsilon);
}


////////////////////////////////////////////////////////////////
//                         base
////////////////////////////////////////////////////////////////

static void base_set_wheel_position(ros::ServiceClient &t, double position=INFINITY) {
  
  wb_motor_set_position(t, position);
}

static void base_set_wheel_velocity(ros::ServiceClient &t, double velocity) {
  
  wb_motor_set_velocity(t, velocity);
}

static void base_set_wheel_speeds_helper(double speeds[4]) {
  int i;
  for (i = 0; i < 4; i++)
  {
    base_set_wheel_velocity(wheels_v[i], speeds[i]);
    //cout<<speeds[i];
}}

void base_init() {
  ros::ServiceClient set_velocity_client,set_position_client;
  
  int i;
  string wheel_name;
  for (i = 0; i < 4; i++) {
    wheel_name="wheel"+to_string(i + 1);

    set_velocity_client = n->serviceClient<webots_ros::set_float>(model_name + "/"+wheel_name+"/set_velocity");
    set_position_client = n->serviceClient<webots_ros::set_float>(model_name + "/"+wheel_name+"/set_position");
    wheels_v[i]=set_velocity_client;
    wheels_p[i]=set_position_client;
    base_set_wheel_velocity(wheels_v[i],0);
    base_set_wheel_position(wheels_p[i],INFINITY);
  }
}

void base_reset() {
  static double speeds[4] = {0.0, 0.0, 0.0, 0.0};
  base_set_wheel_speeds_helper(speeds);
  robot_vx = 0.0;
  robot_vy = 0.0;
  robot_omega = 0.0;
}

void base_forwards() {
  static double speeds[4] = {SPEED, SPEED, SPEED, SPEED};
  base_set_wheel_speeds_helper(speeds);
}

void base_backwards() {
  static double speeds[4] = {-SPEED, -SPEED, -SPEED, -SPEED};
  base_set_wheel_speeds_helper(speeds);
}

void base_turn_left() {
  static double speeds[4] = {-SPEED, SPEED, -SPEED, SPEED};
  base_set_wheel_speeds_helper(speeds);
}

void base_turn_right() {
  static double speeds[4] = {SPEED, -SPEED, SPEED, -SPEED};
  base_set_wheel_speeds_helper(speeds);
}

void base_strafe_left() {
  static double speeds[4] = {SPEED, -SPEED, -SPEED, SPEED};
  base_set_wheel_speeds_helper(speeds);
}

void base_strafe_right() {
  static double speeds[4] = {-SPEED, SPEED, SPEED, -SPEED};
  base_set_wheel_speeds_helper(speeds);
}

void base_move(double vx, double vy, double omega) {
  double speeds[4];
  speeds[0] = 1 / WHEEL_RADIUS * (vx + vy + (LX + LY) * omega);
  speeds[1] = 1 / WHEEL_RADIUS * (vx - vy - (LX + LY) * omega);
  speeds[2] = 1 / WHEEL_RADIUS * (vx - vy + (LX + LY) * omega);
  speeds[3] = 1 / WHEEL_RADIUS * (vx + vy - (LX + LY) * omega);
  base_set_wheel_speeds_helper(speeds);
  printf("Speeds: vx=%.2f[m/s] vy=%.2f[m/s] ω=%.2f[rad/s]\n", vx, vy, omega);
}

void base_forwards_increment() {
  robot_vx += SPEED_INCREMENT;
  robot_vx = robot_vx > MAX_SPEED ? MAX_SPEED : robot_vx;
  base_move(robot_vx, robot_vy, robot_omega);
}

void base_backwards_increment() {
  robot_vx -= SPEED_INCREMENT;
  robot_vx = robot_vx < -MAX_SPEED ? -MAX_SPEED : robot_vx;
  base_move(robot_vx, robot_vy, robot_omega);
}

void base_turn_left_increment() {
  robot_omega += SPEED_INCREMENT;
  robot_omega = robot_omega > MAX_SPEED ? MAX_SPEED : robot_omega;
  base_move(robot_vx, robot_vy, robot_omega);
}

void base_turn_right_increment() {
  robot_omega -= SPEED_INCREMENT;
  robot_omega = robot_omega < -MAX_SPEED ? -MAX_SPEED : robot_omega;
  base_move(robot_vx, robot_vy, robot_omega);
}

void base_strafe_left_increment() {
  robot_vy += SPEED_INCREMENT;
  robot_vy = robot_vy > MAX_SPEED ? MAX_SPEED : robot_vy;
  base_move(robot_vx, robot_vy, robot_omega);
}

void base_strafe_right_increment() {
  robot_vy -= SPEED_INCREMENT;
  robot_vy = robot_vy < -MAX_SPEED ? -MAX_SPEED : robot_vy;
  base_move(robot_vx, robot_vy, robot_omega);
}

void base_goto_init(double time_step) {

  goto_data.v_target.u = 0.0;
  goto_data.v_target.v = 0.0;
  goto_data.alpha = 0.0;
  goto_data.reached = false;
}

void base_goto_set_target(double x, double y, double alpha) {
  
  goto_data.v_target.u = x;
  goto_data.v_target.v = y;
  goto_data.alpha = alpha;
  goto_data.reached = false;
}

void base_goto_run() {
  
  // get sensors

  callbackCalled_gps = false;
  callbackCalled_compass = false;

  ros::Subscriber gps_sub=n->subscribe(model_name+"/gps/values", 1, GPSCallback);
  ros::Subscriber compass_sub=n->subscribe(model_name+"/compass/values", 1, compassCallback);
  //ros::Subscriber camera_recognition_sub = n->subscribe(model_name + "/kinect_color/recognition_objects", 1, cameraRecognitionCallback);
  
  while(!(callbackCalled_gps&&callbackCalled_compass))
    ros::spinOnce();

  // compute 2d vectors
  Vector2 v_gps = {GPSValues[0], GPSValues[1]};
  Vector2 v_front = {compassValues[0], compassValues[1]};
  Vector2 v_right = {-v_front.v, v_front.u};
  Vector2 v_north = {1.0, 0.0};

  // compute distance
  Vector2 v_dir;
  vector2_minus(&v_dir, &goto_data.v_target, &v_gps);
  double distance = vector2_norm(&v_dir);

  // compute absolute angle & delta with the delta with the target angle
  double theta = vector2_angle(&v_front, &v_north);
  double delta_angle = theta - goto_data.alpha;

  // compute the direction vector relatively to the robot coordinates
  // using an a matrix of homogenous coordinates
  Matrix33 transform;
  matrix33_set_identity(&transform);
  transform.a.u = -v_right.u;
  transform.a.v = v_front.u;
  transform.b.u = v_right.v;
  transform.b.v = -v_front.v;
  transform.c.u = v_right.u * v_gps.u - v_right.v * v_gps.v;
  transform.c.v = -v_front.u * v_gps.u + v_front.v * v_gps.v;
  Vector3 v_target_tmp = {goto_data.v_target.u, goto_data.v_target.v, 1.0};
  Vector3 v_target_rel;
  matrix33_mult_vector3(&v_target_rel, &transform, &v_target_tmp);

  // compute the speeds
  double speeds[4] = {0.0, 0.0, 0.0, 0.0};
  // -> first stimulus: delta_angle

  speeds[0] = -delta_angle / M_PI * K1;
  speeds[1] = delta_angle / M_PI * K1;
  speeds[2] = -delta_angle / M_PI * K1;
  speeds[3] = delta_angle / M_PI * K1;

  // -> second stimulus: u coord of the relative target vector
  speeds[0] += v_target_rel.u * K2;
  speeds[1] += v_target_rel.u * K2;
  speeds[2] += v_target_rel.u * K2;
  speeds[3] += v_target_rel.u * K2;

  // -> third stimulus: v coord of the relative target vector
  speeds[0] += -v_target_rel.v * K3;
  speeds[1] += v_target_rel.v * K3;
  speeds[2] += v_target_rel.v * K3;
  speeds[3] += -v_target_rel.v * K3;

  // apply the speeds
  int i;
  for (i = 0; i < 4; i++) {
    speeds[i] /= (K1 + K2 + K2);  // number of stimuli (-1 <= speeds <= 1)
    speeds[i] *= SPEED;           // map to speed (-SPEED <= speeds <= SPEED)

    // added an arbitrary factor increasing the convergence speed
    speeds[i] *= 30.0;
    speeds[i] = bound(speeds[i], -SPEED, SPEED);
  }

  base_set_wheel_speeds_helper(speeds);

  // check if the taget is reached
  if (distance < DISTANCE_TOLERANCE && delta_angle < ANGLE_TOLERANCE && delta_angle > -ANGLE_TOLERANCE)
    goto_data.reached = true;
}

bool base_goto_reached() {
  return goto_data.reached;
}


////////////////////////////////////////////////////////////////
//                         main init
////////////////////////////////////////////////////////////////
/*******************************************************
* Function name ：cmdvelDataCallback
* Description   ：compass回调函数，获取compass消息
* Parameter     ：
        @ values   compass消息值
* Return        ：无
**********************************************************/

bool callbackCalledcmd=false;
void cmdnumDataCallback(const std_msgs::String::ConstPtr &values)
{  // 49 47 39
  if(values->data=="0")
  {
    findobject_id=49;
    callbackCalledcmd=true;
  }
  if(values->data=="1")
  {
    findobject_id=47;
    callbackCalledcmd=true;
  }
  if(values->data=="2")
  {
    findobject_id=39;
    callbackCalledcmd=true;
  }
}


double linear_x=-1,linear_y=-1, angular_z=-1;//暂存的线速度和角速度,
void cmdvelDataCallback(const geometry_msgs::Twist::ConstPtr &value)
{
    angular_z = value->angular.z ;//获取/cmd_vel的角速度,rad/s
    linear_x = value->linear.x ;//获取/cmd_vel的线速度.m/s
    linear_y = value->linear.y;
    //cout<<endl<<linear_x<<" "<<linear_y<<" "<<angular_z<<endl;
    base_move(linear_x,linear_y,angular_z);
    
  
}

static void step() {
  //time_step_client.call(time_step_srv);

  if (time_step_client.call(time_step_srv) && time_step_srv.response.success)
    ROS_INFO("time_step service works.");
  else
    ROS_ERROR("Failed to call service time_step to update robot's time step.");

}

static void passive_wait(double sec) {
  int i=0;
  while(i<sec*1000/TIME_STEP)
  {
    i++;
    time_step_client.call(time_step_srv);
}

}

static void high_level_go_to(double x, double y, double a) {
  base_goto_set_target(x, y, a);
  while (!base_goto_reached()) {
    base_goto_run();
    //step();
  }
  base_reset();
}

static void high_level_grip_box(double y, int level, int column, bool grip) {
  static double h_per_step = 0.002;
  static double box_length = 0.05;
  static double box_gap = 0.01;
  static double platform_height = 0.01;
  static double offset = 0.01;  // security margin

  double x = 0.5 * column * (box_gap + box_length);
  double z = offset + platform_height + (level + 1) * box_length;
  x *= 0.9;  // This fix a small offset that I cannot explain

  if (!grip)
    z += offset;

  // prepare
  arm_set_sub_arm_rotation(ARM5, M_PI_2);
  arm_ik(x, y, 0.20);
  if (grip)
    gripper_release();
  passive_wait(1.0);

  // move the arm down
  double h;
  for (h = 0.2; h > z; h -= h_per_step) {
    arm_ik(x, y, h);
    step();
  }

  passive_wait(0.2);

  // grip or ungrip
  if (grip)
    gripper_set_gap(0.02);
  else
    gripper_release();
  passive_wait(1.0);

  // move the arm up
  for (h = z; h < 0.2; h += h_per_step) {
    arm_ik(x, y, h);
    step();
  }
  arm_set_orientation(ARM_FRONT);
}

static void high_level_go_to_grip(double x, double y, double z, bool grip) {
    
    double h;
    static double h_per_step = 0.004;
    // prepare
    if (grip)
        gripper_release();
    arm_set_sub_arm_rotation(ARM5, M_PI_2);
    arm_ik(x, y, 0.25);
    passive_wait(1);

    for (h = 0.25; h > z; h -= h_per_step) {
    arm_ik(x, y, h);
    step();
  }
    // arm_ik(x, y, z);

    passive_wait(2);
    if (grip)
        gripper_set_gap(0.02);
    else
        gripper_release();
    passive_wait(1);
   for (h = z; h < 0.25; h += h_per_step) {
    arm_ik(x, y, h);
    step();
  }
    // arm_ik(x, y, 0.2);
    // move the arm up
    arm_set_orientation(ARM_FRONT);
    
}


static void high_level_stock(int o, bool stock) {
  arm_set_height(ARM_BACK_PLATE_HIGH);
  arm_set_orientation(Orientation(o));
  passive_wait(4.5);
  if (stock)
    gripper_release();
  else
    gripper_set_gap(0.02);
  passive_wait(1.0);
  arm_set_height(ARM_HANOI_PREPARE);
  passive_wait(3.0);
}

void nav_init(double x,double y,double theta)
{geometry_msgs::PoseWithCovarianceStamped pose_init;

  ros::Publisher pub_nav = n->advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose",10);    
  pose_init.header.stamp = ros::Time::now();
  pose_init.pose.pose.position.x=x;
  pose_init.pose.pose.position.y=y;
  pose_init.pose.pose.orientation.w=cos(theta/2);
  pose_init.pose.pose.orientation.z=sin(theta/2);
  pose_init.pose.covariance={0.0005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001};
  pose_init.header.frame_id= "map";

  pub_nav.publish(pose_init);
  ros::Duration(1).sleep();
  pub_nav.publish(pose_init);
  ros::Duration(1).sleep();
  pub_nav.publish(pose_init);
  ros::Duration(1).sleep();
  pub_nav.publish(pose_init);
}
void nav_goal(double x,double y,double theta){
  geometry_msgs::PoseStamped pose_goal;
  ros::Subscriber gps_sub=n->subscribe(model_name+"/gps/values", 1, GPSCallback);
  ros::Subscriber compass_sub=n->subscribe(model_name+"/compass/values", 1, compassCallback);
  ros::Subscriber cmdvelSub = n->subscribe("/cmd_vel",1,cmdvelDataCallback);
  ros::Publisher pub_nav = n->advertise<geometry_msgs::PoseStamped>("move_base_simple/goal",10);  

  pose_goal.header.stamp = ros::Time::now();
  pose_goal.header.frame_id="map" ;
  pose_goal.pose.position.x=x;
  pose_goal.pose.position.y=y;
  pose_goal.pose.orientation.w=cos(theta/2);
  pose_goal.pose.orientation.z=sin(theta/2);

  pub_nav.publish(pose_goal); 
  ros::Duration(1).sleep();
  pub_nav.publish(pose_goal); 
  ros::Duration(0.25).sleep();
  pub_nav.publish(pose_goal);
  ros::Duration(0.25).sleep();
  pub_nav.publish(pose_goal); 
  ros::Duration(0.25).sleep();
  pub_nav.publish(pose_goal);

  int count=10;
  while(count)
  {
    ros::spinOnce();
    if(angular_z==0&&linear_x==0&&linear_y==0&&
    abs(GPSValues[0]-x)<0.1&&abs(GPSValues[1]-y)<0.1)
    count--;
  }
   
}

static void automatic_behavior() {
 
  double distance_arm0_platform = 0.2;
  double distance_arm0_robot_center = 0.189;
  double distance_origin_platform = 2.50;
  double delta = distance_origin_platform - distance_arm0_platform - distance_arm0_robot_center;

  callbackCalled_recognition = false;
  callbackCalled_rangefinder = false;

  ros::Subscriber gps_sub=n->subscribe(model_name+"/gps/values", 1, GPSCallback);
  ros::Subscriber compass_sub=n->subscribe(model_name+"/compass/values", 1, compassCallback);
  ros::Subscriber range_finder_sub = n->subscribe(model_name + "/kinect_range/range_image", 1, rangeFinderCallback);

  arm_set_height(ARM_HANOI_PREPARE);
  nav_goal(0+0.04,2.08+0.07,M_PI/2);
  //nav_goal(0,2.08+0.05,M_PI/2);
  
  ros::Subscriber yolov5_sub = n->subscribe("/yolo_result_out", 1, yolov5Callback);
  passive_wait(3);
  while(!(callbackCalled_recognition&&callbackCalled_rangefinder))
  {
    cout<<endl<<callbackCalled_recognition<<"  "<<callbackCalled_rangefinder<<endl;
  ros::spinOnce();
  ros::Duration(0.1).sleep();
  }
  // passive_wait(10) ;
  // arm_ik(0, 0.2, 0.2);
  // passive_wait(10) ;
  // ros::Duration(5).sleep();
  // arm_ik(0.2, 0, 0.2);
  // ros::Duration(5).sleep();

  xyd_to_xyz_(int(findobject_point.u),int(findobject_point.v));
  cout<<endl<<world_point.u+0.31-0.156+0.03<<" "<<world_point.v<<" "<< world_point.w-0.02;
  high_level_go_to_grip(-world_point.v,world_point.u+0.31-0.156-0.02,  world_point.w+0.1, true);
  arm_set_height(ARM_FRONT_CARDBOARD_BOX);
  arm_set_sub_arm_rotation(ARM5, M_PI_2);

  nav_init(GPSValues[0], GPSValues[1], atan2(compassValues[0],compassValues[1]));

  ros::Duration(2).sleep();
  cout<<"aaa"<<endl;
  // high_level_grip_box(distance_arm0_platform, 2, 0, true);
  // high_level_stock(ARM_HANOI_PREPARE, true);
  nav_goal(0.01, -delta+0.03, -M_PI / 2);
  high_level_go_to_grip(-world_point.v,world_point.u+0.31-0.156-0.015, world_point.w+0.1, false);
  
  // high_level_grip_box(distance_arm0_platform, 0, 0, false);
  
  // // SRC B1 & B2 => TMP
  // high_level_go_to(goto_info[GOTO_SRC][0], goto_info[GOTO_SRC][1], goto_info[GOTO_SRC][2]);
  // high_level_grip_box(distance_arm0_platform, 1, 1, true);
  // high_level_stock(ARM_FRONT, true);
  // high_level_grip_box(distance_arm0_platform, 1, -1, true);
  // high_level_go_to(goto_info[GOTO_TMP][0], goto_info[GOTO_TMP][1], goto_info[GOTO_TMP][2]);
  // high_level_grip_box(distance_arm0_platform, 0, -1, false);
  // high_level_stock(ARM_FRONT, false);
  // high_level_grip_box(distance_arm0_platform, 0, 1, false);
  // // DST A1 => TMP
  // high_level_go_to(goto_info[GOTO_DST][0], goto_info[GOTO_DST][1], goto_info[GOTO_DST][2]);
  // high_level_grip_box(distance_arm0_platform, 0, 0, true);
  // high_level_go_to(goto_info[GOTO_TMP][0], goto_info[GOTO_TMP][1], goto_info[GOTO_TMP][2]);
  // high_level_grip_box(distance_arm0_platform, 1, 0, false);
  // // SRC C1-C2-C3 => DST
  // high_level_go_to(goto_info[GOTO_SRC][0], goto_info[GOTO_SRC][1], goto_info[GOTO_SRC][2]);
  // high_level_grip_box(distance_arm0_platform, 0, -2, true);
  // high_level_stock(ARM_FRONT_LEFT, true);
  // high_level_grip_box(distance_arm0_platform, 0, 0, true);
  // high_level_stock(ARM_FRONT_RIGHT, true);
  // high_level_grip_box(distance_arm0_platform, 0, 2, true);
  // high_level_go_to(goto_info[GOTO_DST][0], goto_info[GOTO_DST][1], goto_info[GOTO_DST][2]);
  // high_level_grip_box(distance_arm0_platform, 0, 2, false);
  // high_level_stock(ARM_FRONT_RIGHT, false);
  // high_level_grip_box(distance_arm0_platform, 0, 0, false);
  // high_level_stock(ARM_FRONT_LEFT, false);
  // high_level_grip_box(distance_arm0_platform, 0, -2, false);
  // // TMP A1 => SRC
  // high_level_go_to(goto_info[GOTO_TMP][0], goto_info[GOTO_TMP][1], goto_info[GOTO_TMP][2]);
  // high_level_grip_box(distance_arm0_platform, 1, 0, true);
  // high_level_go_to(goto_info[GOTO_SRC][0], goto_info[GOTO_SRC][1], goto_info[GOTO_SRC][2]);
  // high_level_grip_box(distance_arm0_platform, 0, 0, false);
  // // TMP B1 & B2 => DST
  // high_level_go_to(goto_info[GOTO_TMP][0], goto_info[GOTO_TMP][1], goto_info[GOTO_TMP][2]);
  // high_level_grip_box(distance_arm0_platform, 0, 1, true);
  // high_level_stock(ARM_FRONT, true);
  // high_level_grip_box(distance_arm0_platform, 0, -1, true);
  // high_level_go_to(goto_info[GOTO_DST][0], goto_info[GOTO_DST][1], goto_info[GOTO_DST][2]);
  // high_level_grip_box(distance_arm0_platform, 1, -1, false);
  // high_level_stock(ARM_FRONT, false);
  // high_level_grip_box(distance_arm0_platform, 1, 1, false);
  // // SRC A1 => DST
  // high_level_go_to(goto_info[GOTO_SRC][0], goto_info[GOTO_SRC][1], goto_info[GOTO_SRC][2]);
  // high_level_grip_box(distance_arm0_platform, 0, 0, true);
  // high_level_go_to(goto_info[GOTO_DST][0], goto_info[GOTO_DST][1], goto_info[GOTO_DST][2]);
  // high_level_grip_box(distance_arm0_platform, 2, 0, false);
  // end behavior
  arm_reset();
  //high_level_go_to(0.0, 0.0, 0.0);
}

int main_init()
{
  //////////////////////////////////////////init
  string controllerName;

  // 订阅webots中所有可用的model_name
  ros::Subscriber name_sub = n->subscribe("model_name", 100, modelNameCallback);
  while (model_count == 0 || model_count < name_sub.getNumPublishers()) {
    ros::spinOnce();
    ros::spinOnce();
    ros::spinOnce();
  }
  ros::spinOnce();
  name_sub.shutdown();

  // 服务订阅time_step和webots保持同步
  time_step_client = n->serviceClient<webots_ros::set_int>(model_name+"/robot/time_step");
  time_step_srv.request.value = TIME_STEP;
  
/////////////////////////////////////////GPS enable
  ros::ServiceClient enable_gps_client;          
  webots_ros::set_int gps_Srv;
  ros::Subscriber sub_GPS_32; 

  enable_gps_client = n->serviceClient<webots_ros::set_int>(model_name+"/gps/enable"); // 使能GPS服务
  gps_Srv.request.value = TIME_STEP;

  // 判断gps使能服务是否成功
  if (enable_gps_client.call(gps_Srv) && gps_Srv.response.success) {
      ROS_INFO("gps enabled.");
  } else {
      if (!gps_Srv.response.success)
      ROS_ERROR("Failed to enable gps.");
      return 1;
  }
  sub_GPS_32.shutdown();
  enable_gps_client.shutdown();

/////////////////////////////////////////// camera enable
  ros::ServiceClient enable_camera_client;
  webots_ros::set_int camera_srv;
  ros::Subscriber sub_camera_color;

  enable_camera_client = n->serviceClient<webots_ros::set_int>(model_name + "/kinect_color/enable");
  camera_srv.request.value = TIME_STEP;

  if (enable_camera_client.call(camera_srv) && camera_srv.response.success) {
    ROS_INFO("Camera enabled.");
  } else {
    if (camera_srv.response.success == -1)
      ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable camera.");
    return 1;
  }

  sub_camera_color.shutdown();
  enable_camera_client.shutdown();


////////////////////////////////////////////compass enable
  ros::ServiceClient set_compass_client;
  webots_ros::set_int compass_srv;
  ros::Subscriber sub_compass_32;

  set_compass_client = n->serviceClient<webots_ros::set_int>(model_name + "/compass/enable");
  compass_srv.request.value = TIME_STEP;

  if (set_compass_client.call(compass_srv) && compass_srv.response.success) {
    ROS_INFO("Compass enabled.");
  } else {
    if (compass_srv.response.success == -1)
      ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable compass.");
    return 1;
  }

  sub_compass_32.shutdown();
  set_compass_client.shutdown();

/////////////////////////////////////////range_finder enable
  ros::ServiceClient set_range_finder_client;
  webots_ros::set_int range_finder_srv;
  ros::Subscriber sub_range_finder_color;

  set_range_finder_client = n->serviceClient<webots_ros::set_int>(model_name + "/scan/enable");
  range_finder_srv.request.value = TIME_STEP;

  if (set_range_finder_client.call(range_finder_srv) && range_finder_srv.response.success) {
    ROS_INFO("scan enabled.");
  } else {
    if (!range_finder_srv.response.success)
      ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable range-finder.");
    return 1;
  }

  set_range_finder_client = n->serviceClient<webots_ros::set_int>(model_name + "/kinect_range/enable");
  range_finder_srv.request.value = TIME_STEP;

  if (set_range_finder_client.call(range_finder_srv) && range_finder_srv.response.success) {
    ROS_INFO("Range-finder enabled.");
  } else {
    if (!range_finder_srv.response.success)
      ROS_ERROR("Sampling period is not valid.");
    ROS_ERROR("Failed to enable range-finder.");
    return 1;
  }

  sub_range_finder_color.shutdown();
  set_range_finder_client.shutdown();


  return 0;
}

////////////////////////////////////////////////////////////////
//                         main run
////////////////////////////////////////////////////////////////

int main(int argc, char **argv) {

  setlocale(LC_ALL, ""); // 用于显示中文字符
  // 在ROS网络中创建一个名为robot_init的节点
  ros::init(argc, argv, "my_robot", ros::init_options::AnonymousName);
  n = new ros::NodeHandle;
  // 截取退出信号
  main_init();
  signal(SIGINT, quit);

  base_init();
  base_goto_init(TIME_STEP);
  arm_init();
  gripper_init();

  // ros::Subscriber cmdvelSub = n->subscribe("/cmd_vel_v",1,cmdvelDataCallback);
  
  // while(1)
  // {
  //   ros::spinOnce();
  //   passive_wait(0.01);
  // }
  
  callbackCalledcmd=false;
  ros::Subscriber cmdnumSub = n->subscribe("/cmd_num",1,cmdnumDataCallback);
  ros::Subscriber compass_sub=n->subscribe(model_name+"/compass/values", 1, compassCallback);

  while(!callbackCalledcmd)
  {
    ros::spinOnce();
    passive_wait(0.1);
    //cout<<compassValues[0]<<" "<<compassValues[1]<<endl;
  }

  nav_init(0, -2, M_PI / 2);
  automatic_behavior();

  return 0;
}

    

    

    
